#include<stdio.h>
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <sched.h> /* sched_setscheduler() */




#include <sys/mman.h>




#include <pthread.h>
#include <math.h>
#define PI 3.1415926535898
#include"ecrt.h"

/****************************************************************************/

/** Task period in ns. */
#define PERIOD_NS   (1000000)

#define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is
                                     guranteed safe to access without
                                     faulting */

/****************************************************************************/

/* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)

#define     CLOCK_TO_USE         CLOCK_REALTIME
#define     NSEC_PER_SEC         (1000000000L)
#define     TIMESPEC2NS(T)       ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)


/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};

static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

static ec_slave_config_t *sc_cooldrive0 = NULL;
static ec_slave_config_state_t sc_cooldrive0_state = {};
static ec_slave_config_t *sc_cooldrive1 = NULL;
static ec_slave_config_state_t sc_cooldrive1_state = {};
/****************************************************************************/

typedef struct{
	unsigned int cntlwd;
	unsigned int tarpos;
	unsigned int modper;
	unsigned int DB1;
	unsigned int status;
	unsigned int actpos;
	unsigned int moddis;
	unsigned int DB2;
	unsigned int errcode;
	unsigned int errflo;

}Robot_Aix;


// process data
#define CoolDrivePos0  1, 0
#define CoolDrivePos1  2, 1

#define CoolDrive 0x00000748, 0x0000000a
//offsets for PDO entries

static Robot_Aix A1,A2,A3,A4;

/*offsets for PDO entries
static unsigned int  cntlwd1,cntlwd2,cntlwd3,cntlwd4;
static unsigned int  tarpos1,tarpos2,tarpos3,tarpos4;
static unsigned int  modper1,modper2,modper3,modper4;
static unsigned int  DB1_1,DB1_2,DB1_3,DB1_4;
static unsigned int  status1,status2,status3,status4;
static unsigned int  actpos1,actpos2,actpos3,actpos4;
static unsigned int  acttor1,acttor2,acttor3,acttor4;
static unsigned int  moddis1,moddis2,moddis3,moddis4;
static unsigned int  DB2_1,DB2_2,DB2_3,DB2_4;
static unsigned int  errcode1,errcode2,errcode3,errcode4;
static unsigned int  errflo1,errflo2,errflo3,errflo4;
*************************/
static int cur_status;
static int cur_mode;
//process data
const static ec_pdo_entry_reg_t domainServoOutput_regs[] = {
	{CoolDrivePos0, CoolDrive, 0x6040, 0x00,&A1.cntlwd,NULL},
	{}
};


const static ec_pdo_entry_reg_t domainServoInput_regs[] = {
	{CoolDrivePos0, CoolDrive, 0x6041, 0x00, &A1.status,NULL},
	{}
};

/*/ process data
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6040, 0x00, &cntlwd1, NULL},
    {CoolDrivePos0, CoolDrive, 0x607a, 0x00, &tarpos1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6060, 0x00, &modper1, NULL},
    {CoolDrivePos0, CoolDrive, 0x27fe, 0x00, &DB1_1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6840, 0x00, &cntlwd2, NULL},
    {CoolDrivePos0, CoolDrive, 0x687a, 0x00, &tarpos2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6860, 0x00, &modper2, NULL},
    {CoolDrivePos0, CoolDrive, 0x2ffe, 0x00, &DB1_2, NULL},
    {CoolDrivePos1, CoolDrive, 0x6040, 0x00, &cntlwd3, NULL},
    {CoolDrivePos1, CoolDrive, 0x607a, 0x00, &tarpos3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6060, 0x00, &modper3, NULL},
    {CoolDrivePos1, CoolDrive, 0x27fe, 0x00, &DB1_3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6840, 0x00, &cntlwd4, NULL},
    {CoolDrivePos1, CoolDrive, 0x687a, 0x00, &tarpos4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6860, 0x00, &modper4, NULL},
    {CoolDrivePos1, CoolDrive, 0x2ffe, 0x00, &DB1_4, NULL},
    {}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6041, 0x00, &status1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6064, 0x00, &actpos1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6077, 0x00, &acttor1, NULL},    
    {CoolDrivePos0, CoolDrive, 0x6061, 0x00, &moddis1, NULL},
    {CoolDrivePos0, CoolDrive, 0x27ff, 0x00, &DB2_1, NULL},
    {CoolDrivePos0, CoolDrive, 0x603f, 0x00, &errcode1, NULL},
    {CoolDrivePos0, CoolDrive, 0x60f4, 0x00, &errflo1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6841, 0x00, &status2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6864, 0x00, &actpos2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6877, 0x00, &acttor2, NULL},    
    {CoolDrivePos0, CoolDrive, 0x6861, 0x00, &moddis2, NULL},
    {CoolDrivePos0, CoolDrive, 0x2fff, 0x00, &DB2_2, NULL},
    {CoolDrivePos0, CoolDrive, 0x683f, 0x00, &errcode2, NULL},
    {CoolDrivePos0, CoolDrive, 0x68f4, 0x00, &errflo2, NULL},
    {CoolDrivePos1, CoolDrive, 0x6041, 0x00, &status3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6064, 0x00, &actpos3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6077, 0x00, &acttor3, NULL},    
    {CoolDrivePos1, CoolDrive, 0x6061, 0x00, &moddis3, NULL},
    {CoolDrivePos1, CoolDrive, 0x27ff, 0x00, &DB2_3, NULL},
    {CoolDrivePos1, CoolDrive, 0x603f, 0x00, &errcode3, NULL},
    {CoolDrivePos1, CoolDrive, 0x60f4, 0x00, &errflo3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6841, 0x00, &status4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6864, 0x00, &actpos4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6877, 0x00, &acttor4, NULL},    
    {CoolDrivePos1, CoolDrive, 0x6861, 0x00, &moddis4, NULL},
    {CoolDrivePos1, CoolDrive, 0x2fff, 0x00, &DB2_4, NULL},
    {CoolDrivePos1, CoolDrive, 0x683f, 0x00, &errcode4, NULL},
    {CoolDrivePos1, CoolDrive, 0x68f4, 0x00, &errflo4, NULL},
    {}
};
*/

